#include <string>
#include <fstream>
#include <cmath>
#include <sstream>
#include "Universe.h"
#include "AbsThreads.h"
#include "Genome.h"
#include "Logger.h"

#define LUT_SIZE		100
#define ELITE_BLLI		10
#define CYCLE_DURATION	1
#define REPEAT_LUT		3
#define DISTANCE_MAX	100
#define DISTANCE_MIN	0.01
#define MAX_TORQUE		10000
#define MAX_DISTANCE_UNIT_COST		MAX_TORQUE / DISTANCE_MIN
#define MAX_LEGS_COLLISION_SCORE	REPEAT_LUT * LUT_SIZE * 6
#define MAX_BODY_COLLISION_SCORE	REPEAT_LUT * LUT_SIZE
#define MAX_TILT_SCORE				REPEAT_LUT * LUT_SIZE * 180
#define MAX_TRAJECTORY_SCORE		REPEAT_LUT * LUT_SIZE * DISTANCE_MAX
#define MAX_DIRECTION_SCORE			0
#define MAX_GRAVITY_SCORE			REPEAT_LUT * LUT_SIZE
#define MAX_VERTICAL_SCORE			REPEAT_LUT * LUT_SIZE * 9

inline std::string FloatToString(float number)
{
	std::ostringstream buff;
	buff << number;
	return buff.str();
}

inline float log2(float n)
{
	if (n <= 0.f)
		return 0.f;
	return (std::log(n) / std::log(2.f)) / 32;
}

inline bool	pnpoly(int nvert, float *vertx, float *verty, float testx, float testy)
{
	int i, j;
	bool c = false;
	for (i = 0, j = nvert-1; i < nvert; j = i++)
	{
		if (((verty[i]>testy) != (verty[j]>testy)) &&
			(testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]))
			c = !c;
	}
	return c;
}

inline float Pythagore(float x1, float x2, float y1, float y2)
{
	float a, b, c;

	a = x2 -x1;
	b = y2 - y1;
	c = std::sqrt((a * a) + (b * b));
	return c;
}

Universe::Universe(const std::list<Blli*>& _initPopulation, const std::list<ApiBlli::Robot*>& _robots, std::map<int, int> _symmetry)
{
	// Sauvegarde des parametres
	this->_Population = _initPopulation;
	this->_PopulationSize = _initPopulation.size();
	this->_Robots = _robots;
	this->_Symmetry = _symmetry;

	// Initialisation des mutex
	this->_MutexGetNextBlli.CreateMutex();
	this->_MutexCloseBlli.CreateMutex();

	// Initialisation des variables
	this->_TheBest = NULL;
	this->_PopulationEvaluated.clear();
	this->_PopulationEvaluatedAndSortedByScore.clear();
}

Universe::~Universe(void)
{
}

void	Universe::CreateLife()
{
	int generationCount = 1;
	do
	{
		EvalPopulation();
		std::ostringstream filename;
		filename << "BestBlli_" << generationCount << ".txt";
		std::ofstream file(filename.str(), std::ios::out);
		if (file)
		{
			for (int i = 0; i < 100; ++i)
			{
				for (int j = 0; j < 6; ++j)
				{
					for (int k = 0; k < 3; ++k)
					{
						std::ostringstream line;
						line << this->_TheBest->GetIndex(this->_Symmetry[j], k, i) << ":" << this->_TheBest->GetSpeed(this->_Symmetry[j], k, i) << ";";
						file << line.str();
					}
				}
				file << std::endl;
			}
			file.close();
		}
		this->_PopulationEvaluatedAndSortedByScore = this->_PopulationEvaluated;
		this->_PopulationEvaluatedAndSortedByScore.sort(SortByScore());
		for (unsigned int i = 0; i < (this->_PopulationEvaluated.size() - ELITE_BLLI) / 2; ++i)
		{
			Blli* mother = NULL;
			Blli* father = NULL;
			Selection(&mother, &father);
			this->_Population.push_back(CrossBlli(father, mother));
			this->_Population.push_back(CrossBlli(mother, father));
		}
		// Pour faire de l'elitisme
		std::list<Blli*>::reverse_iterator itrb = this->_PopulationEvaluatedAndSortedByScore.rbegin();
		std::list<Blli*>::reverse_iterator itre = this->_PopulationEvaluatedAndSortedByScore.rend();

		int count = 0;
		for (; itrb != itre; ++itrb)
		{
			if (count >= ELITE_BLLI)
			{
				delete (*itrb);
			}
			else
			{
				this->_Population.push_back((*itrb));
			}
			count++;
		}

		// Reinitialisation des valeurs
		this->_TheBest = NULL;
		this->_PopulationEvaluatedAndSortedByScore.clear();
		this->_PopulationEvaluated.clear();
		generationCount++;
	} while (1);
}

void	Universe::EvalPopulation()
{
	AbsThreads *threads = new AbsThreads[this->_Robots.size()];
	std::list<ApiBlli::Robot*>::iterator itb = this->_Robots.begin();
	std::list<ApiBlli::Robot*>::iterator ite = this->_Robots.end();

	int i = 0;
	for (; itb != ite; ++itb)
	{
		threads[i].AbsCreateThread(EvalPopulationRoutine, new EvalRoutineParameters(this, (*itb)));
		++i;
	}
	for (int i = 0; i < this->_Robots.size(); ++i)
	{
		threads[i].AbsJoinThread();
	}
	delete [] threads;
}

void*	Universe::EvalPopulationRoutine(void* param)
{
	EvalRoutineParameters* context = (EvalRoutineParameters*)param;
	Universe* universe = context->_Universe;
	ApiBlli::Robot* robot = context->_Robot;
	delete context;

	Blli* blli = NULL;
	while ((blli = universe->GetNextBlli()) != NULL)
	{
		robot->Init();
		std::map<int, Measure*>	measures;
		for (int i = 0; i < REPEAT_LUT; ++i)
		{
			for (int cycle = 0; cycle < 100; ++cycle)
			{
				// Symetrie
				std::map<int, int>::iterator itb = universe->_Symmetry.begin();
				std::map<int, int>::iterator ite = universe->_Symmetry.end();
				for (; itb != ite; ++ itb)
				{
					robot->SetIndex(itb->first, ApiBlli::Leg::FINGER, blli->GetIndex(itb->second, ApiBlli::Leg::FINGER, cycle), blli->GetSpeed(itb->second, ApiBlli::Leg::FINGER, cycle));
					robot->SetIndex(itb->first, ApiBlli::Leg::ARM, blli->GetIndex(itb->second, ApiBlli::Leg::ARM, cycle), blli->GetSpeed(itb->second, ApiBlli::Leg::ARM, cycle));
					robot->SetIndex(itb->first, ApiBlli::Leg::SHOULDER, blli->GetIndex(itb->second, ApiBlli::Leg::SHOULDER, cycle), blli->GetSpeed(itb->second, ApiBlli::Leg::SHOULDER, cycle));
				}
				// On attend la fin des mouvement
				robot->Wait(CYCLE_DURATION);
				// On fait les mesures
				Measure* measure = new Measure();
				measure->_BodyTouchingGround = robot->IsTuchingGround();
				measure->_BodyPosition = robot->GetPosition();
				measure->_GyroValues = robot->GetGyroValues();
				for (int leg = 0; leg < 6; ++leg)
				{
					measure->_Colision[leg] = robot->IsInColision(leg);
					measure->_LegTouchingGround[leg] = robot->IsTuchingGround(leg);
					measure->_FingerPostion[leg] = robot->GetFingerPosition(leg);
					for (int servo = 0; servo < 3; ++servo)
					{
						measure->_Torque[leg * 3 + servo] = robot->GetTorque(leg, servo);
						measure->_MovementEnd[leg * 3 + servo] = std::abs(robot->GetIndex(leg, servo) - blli->GetIndex(universe->_Symmetry[leg], servo, cycle));
						measure->_ServoPostion[leg * 3 + servo] = robot->GetPosition(leg, servo);
					}
				}
				measures[i * 100 + cycle] = measure;
			}
		}
		std::string log = "";
		blli->SetScore(EvalScore(measures, log));
		Logger::GetInstance()->WriteLine(log);
#ifdef DEBUG
		std::cout << "Score: " << blli->GetScore() << std::endl;
#endif
		std::map<int, Measure*>::iterator itb = measures.begin();
		std::map<int, Measure*>::iterator ite = measures.end();
		for (; itb != ite; ++itb)
		{
			delete itb->second;
		}
		measures.clear();
		universe->CloseBlli(blli);
	}
	return NULL;
}

Blli*	Universe::GetNextBlli()
{
	Blli* next = NULL;
	this->_MutexGetNextBlli.LockMutex();
	if (this->_Population.size() > 0)
	{
		next = *(this->_Population.begin());
		this->_Population.pop_front();
	}
	this->_MutexGetNextBlli.UnLockMutex();
	return next;
}

void	Universe::CloseBlli(Blli* _blli)
{
	this->_MutexCloseBlli.LockMutex();
	if (this->_TheBest == NULL)
	{
		this->_TheBest = _blli;
	}
	else
	{
		if (this->_TheBest->GetScore() < _blli->GetScore())
		{
			this->_TheBest = _blli;
		}
	}
#ifdef DEBUG
	std::cout << "Current Best is: " << this->_TheBest->GetScore() << std::endl;
#endif
	this->_PopulationEvaluated.push_back(_blli);
	this->_MutexCloseBlli.UnLockMutex();
}

void	Universe::Selection(Blli** _mother, Blli** _father)
{
	*_mother = RouletteWheel();
	*_father = RouletteWheel();
}

Blli*	Universe::RouletteWheel()
{
	int S = 0;
	int r = -1;
	int s = 0;

	std::list<Blli*>::iterator itbSorted = this->_PopulationEvaluatedAndSortedByScore.begin();
	std::list<Blli*>::iterator iteSorted = this->_PopulationEvaluatedAndSortedByScore.end();
	// On attribue son rang a chaque individu
	int rank = 0;
	for (; itbSorted != iteSorted; ++itbSorted)
	{
		(*itbSorted)->SetRank(rank);
		++rank;
	}

	std::list<Blli*>::iterator itb = this->_PopulationEvaluated.begin();
	std::list<Blli*>::iterator ite = this->_PopulationEvaluated.end();
	for (; itb != ite; ++itb)
	{
		S += (*itb)->GetRank();
	}

	r = rand() % S;
	itb = this->_PopulationEvaluated.begin();
	for (; itb != ite; ++itb)
	{
		s += (*itb)->GetRank();
		if (s > r)
		{
#ifdef DEBUG
			std::cout << "rank choisi: " << (*itb)->GetRank() << '(' << (*itb)->GetScore() << ')' << std::endl;
#endif
			return (*itb);
		}
	}
	return *(this->_PopulationEvaluated.begin());
}

Blli*	Universe::CrossBlli(Blli* mother, Blli* father)
{
	std::map<int, float**> MotherLuts = mother->GetLuts();
	std::map<int, float**> FatherLuts = father->GetLuts();
	std::map<int, float**>::iterator itb = MotherLuts.begin();
	std::map<int, float**>::iterator ite = MotherLuts.end();

	std::map<int, float**> SonLuts;
	for (; itb != ite; ++itb)
	{
		float** lut;
		lut = new float*[100];
		for (int j = 0; j < 100; ++j)
			lut[j] = new float[2];
		int a = rand() % 100;
		int b = rand() % 100;

		// On swap les valeurs si b < a
		if (b < a)
		{
			std::swap(a, b);
		}
		// On croise les tables des parents
		for (int j = 0; j < a; ++j)
		{
			lut[j][Blli::INDEX] = MotherLuts[itb->first][j][Blli::INDEX];
			lut[j][Blli::SPEED] = MotherLuts[itb->first][j][Blli::SPEED];
		}
		for (int j = a; j < b; ++j)
		{
			lut[j][Blli::INDEX] = FatherLuts[itb->first][j][Blli::INDEX];
			lut[j][Blli::SPEED] = FatherLuts[itb->first][j][Blli::SPEED];
		}
		for (int j = b; j < 100; ++j)
		{
			lut[j][Blli::INDEX] = MotherLuts[itb->first][j][Blli::INDEX];
			lut[j][Blli::SPEED] = MotherLuts[itb->first][j][Blli::SPEED];
		}
		SonLuts[itb->first] = lut;
	}
	return Mutate(SonLuts);
}

Blli*	Universe::Mutate(std::map<int, float**>& _luts)
{
	if (rand() % 100 < 2)
	{
		std::map<int, float**>::iterator itb = _luts.begin();
		std::map<int, float**>::iterator ite = _luts.end();

		int servo = 0;
		for (; itb != ite; ++itb)
		{
			for (int i = 0; i < 100; ++i)
			{
				if (rand() % 100 < 2)
				{
					if (rand() % 2)
					{
						float val = itb->second[i][Blli::INDEX] + (rand() % 100);
						itb->second[i][Blli::INDEX] = (val > Genome::GetIndexMax(servo)) ? Genome::GetIndexMax(servo) : val;
						itb->second[i][Blli::SPEED] = itb->second[i][Blli::SPEED] + (float)(rand() % 100);
					}
					else
					{
						float val = itb->second[i][Blli::INDEX] - (rand() % 100);
						itb->second[i][Blli::INDEX] = (val > Genome::GetIndexMin(servo)) ? Genome::GetIndexMin(servo) : val;
						itb->second[i][Blli::SPEED] = itb->second[i][Blli::SPEED] - (float)(rand() % 100);
					}
				}
			}
			servo = ((servo > 2 ) ? 0 : servo + 1);
		}
	}
	return new Blli(_luts);
}

float	Universe::EvalScore(const std::map<int, Measure*>& measures, std::string& log)
{
	//float secRange = EvalRobotSecurity(measures);
	//float gwRange = EvalGoodWay(measures);
	//float baseScore = ((float)4.0 - secRange) * (float)25.0 + ((float)2.0 - gwRange) * (float)12.5;
	//return baseScore + EvalEfficiency(measures);
	float a = EvalRobotSecurity(measures) * (float)(100);
	log += FloatToString(a);
	log += ";";
	float b = EvalGoodWay(measures) * (float)(10);
	log += FloatToString(b);
	log += ";";
	float c = EvalEfficiency(measures) * (float)(1);
	log += FloatToString(c);
	log += ";";
	log += FloatToString((a + b + c));
	log += ";";
	return a + b + c;
}

float	Universe::CalcDistance(const std::map<int, Measure*>& measures)
{
	float x1, x2, y1, y2;

	x1 = measures.begin()->second->_BodyPosition->x;
	x2 = measures.rbegin()->second->_BodyPosition->x;
	y1 = measures.begin()->second->_BodyPosition->y;
	y2 = measures.rbegin()->second->_BodyPosition->y;
	return Pythagore(x1, x2, y1, y2);
}

float	Universe::CalcDistanceUnitCost(const std::map<int, Measure*>& measures)
{
	float dist = CalcDistance(measures);

	float totalCost = 0.f;

	std::map<int, Measure*>::const_iterator itb = measures.begin();
	std::map<int, Measure*>::const_iterator ite = measures.end();
	for (; itb != ite; ++itb)
	{
		for (int i = 0; i < 18; ++i)
		{
			totalCost += itb->second->_Torque[i];
		}
	}
	if (dist <= DISTANCE_MIN)
		return MAX_DISTANCE_UNIT_COST;
	return totalCost / dist;
}

float	Universe::EvalEfficiency(const std::map<int, Measure*>& measures)
{
	float cd = (float)(1.0) - CalcDistance(measures) / (float)(DISTANCE_MAX);
	float cduc = (float)(1.0) - CalcDistanceUnitCost(measures) / (float)(MAX_DISTANCE_UNIT_COST);
	//return (log2(DISTANCE_MAX - CalcDistance(measures)) * 8) + (log2(CalcDistanceUnitCost(measures)) * 2);
	return ((cd * (float)(8.0)) + (cduc * (float)(2.0))) / (float)(10.0);
}

float	Universe::CalcTrajectoryFitness(const std::map<int, Measure*>& measures)
{
	float yStart = measures.begin()->second->_BodyPosition->y;
	float ecart = 0.f;
	std::map<int, Measure*>::const_iterator itb = measures.begin();
	std::map<int, Measure*>::const_iterator ite = measures.end();
	for (; itb != ite; ++itb)
	{
		itb->second->_BodyPosition->y;
		ecart += std::abs(yStart - itb->second->_BodyPosition->y);
	}
	return ecart;
}

float	Universe::CalcDirectionFitness(const std::map<int, Measure*>& measures)
{
	return 0.f;
}

float	Universe::EvalGoodWay(const std::map<int, Measure*>& measures)
{
	float ctf = (float)(1.0) - CalcTrajectoryFitness(measures) / (float)(MAX_TRAJECTORY_SCORE);
	//float cdf = CalcDirectionFitness(measures) / MAX_DIRECTION_SCORE
	return ctf /* + cdf */; // Diviser par 2 quand 2 valeurs
}

float	Universe::CalcGravityCenter(const std::map<int, Measure*>& measures)
{
	float nbTimeNotInPoly = 0.f;
	std::map<int, Measure*>::const_iterator itb = measures.begin();
	std::map<int, Measure*>::const_iterator ite = measures.end();
	for (; itb != ite; ++itb)
	{
		int nvert = 0;
		float vertx[6], verty[6];
		for (int i = 0; i < 6; ++i)
		{
			if (itb->second->_LegTouchingGround[i])
			{
				vertx[nvert] = itb->second->_FingerPostion[i]->x;
				verty[nvert] = itb->second->_FingerPostion[i]->y;
				nvert++;
			}
		}
		bool InPoly = false;
		if (nvert >=3)
		{
			InPoly = pnpoly(nvert, vertx, verty, itb->second->_BodyPosition->x, itb->second->_BodyPosition->y);
		}
		if (!InPoly)
			nbTimeNotInPoly++;
	}
	return nbTimeNotInPoly;
}

float	Universe::CalcVerticalVariation(const std::map<int, Measure*>& measures)
{
	float total = 0.f;
	std::map<int, Measure*>::const_iterator itb = measures.begin();
	std::map<int, Measure*>::const_iterator ite = measures.end();
	for (; itb != ite; ++itb)
	{
		total += itb->second->_GyroValues->LinearAccelerations[2];
	}
	return total;
}

float	Universe::CalcHorizontalTilt(const std::map<int, Measure*>& measures)
{
	float total = 0.f;
	std::map<int, Measure*>::const_iterator itb = measures.begin();
	std::map<int, Measure*>::const_iterator ite = measures.end();
	for (; itb != ite; ++itb)
	{
		total += std::abs(itb->second->_GyroValues->Angles[0]);
		total += std::abs(itb->second->_GyroValues->Angles[1]);
	}
	return total;
}

float	Universe::EvalBodyStability(const std::map<int, Measure*>& measures)
{
	float cvv = (float)(1.0) - CalcVerticalVariation(measures) / (float)(MAX_VERTICAL_SCORE);
	float cht = (float)(1.0) - CalcHorizontalTilt(measures) / (float)(MAX_TILT_SCORE);
	return (cvv + cht) / (float)(2.0);
}

float	Universe::CalcLegsCollisions(const std::map<int, Measure*>& measures)
{
	float total = 0.f;
	std::map<int, Measure*>::const_iterator itb = measures.begin();
	std::map<int, Measure*>::const_iterator ite = measures.end();
	for (; itb != ite; ++itb)
	{
		for (int i = 0; i < 6; ++i)
		{
			total += ((itb->second->_Colision[i]) ? 1 : 0);
		}
	}
	return total;
}

float	Universe::CalcBodyCollisions(const std::map<int, Measure*>& measures)
{
	float total = 0.f;
	std::map<int, Measure*>::const_iterator itb = measures.begin();
	std::map<int, Measure*>::const_iterator ite = measures.end();
	for (; itb != ite; ++itb)
	{
		total += ((itb->second->_BodyTouchingGround) ? 1 : 0);
	}
	return total;
}

float	Universe::EvalRobotSecurity(const std::map<int, Measure*>& measures)
{
	float a, b, c;
	a = (float)(1.0) - CalcLegsCollisions(measures) / (float)(MAX_LEGS_COLLISION_SCORE);
	b = (float)(1.0) - CalcBodyCollisions(measures) / (float)(MAX_BODY_COLLISION_SCORE);
	c = (float)(1.0) - EvalBodyStability(measures) / (float)(MAX_GRAVITY_SCORE);
	return (a + b + c) / (float)(3.0);
}
